#!/usr/bin/env python3
# coding:utf-8
import os
import sys
import copy
import socket
import struct
import time
from drawObs import draw_obs

class recv_obs():
    def __init__(self,ip="127.0.0.1",port=6699,hz=8):
        # 1.创建一个udp套接字
        self.ip = ip
        self.port = port
        self.rate_time = 1/hz
        self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.udp_socket.settimeout(2)
        # 2.draw 绘画
        self.draw_impl = draw_obs(18, 16)
        self.draw_impl.draw_show_wait_key()

    def loop_run(self):
        while True:
            self.draw_impl.draw_reset()
            # 发送请求: get_obs
            self.udp_socket.sendto("get_obs".encode('utf-8'), (self.ip, self.port))
            try:
                recv_data = self.udp_socket.recvfrom(1024)
                self.parse_obs_data(recv_data[0])
            except Exception as e:
                print("error",e)
                # self.draw_impl.draw_show_wait_key()
                # continue
            self.draw_impl.draw_show_wait_key()
            time.sleep(self.rate_time)

    def close_network(self):
        # 3.关闭套接字
        self.udp_socket.close()

    def parse_obs_data(self,data):
        self.draw_impl.draw_reset()
        if data[0] == ord('R') and data[1] == ord('E') and data[4] == 0xc1 \
            and data[5] == 0x05 and data[6] == 0x19:
            offset = 7      # head偏移
            obs_num,obs_flag,obs_pixel = struct.unpack("BBB",data[offset:offset+3])
            print(time.time())
            print("recv obs num:%d, flag=%d, pixel:%d"%(obs_num,obs_flag,obs_pixel))

            offset+=3       # obs_head偏移
            for i in range(obs_num):
                x,y = struct.unpack("bB",data[offset:offset+2])
                # print("x=%d y=%d"%(x,y))
                self.draw_impl.draw_rectangle(x,y,obs_pixel)
                offset+=2   # 下一个障碍物坐标偏移
                # self.draw_impl.draw_show_wait_key()
        else:
            print("not obs data")

def help():
    print("usage: example")
    print("%s ip  port"%(os.path.basename(__file__)))
    print("%s 192.168.1.56 6699"%(os.path.basename(__file__)))

if __name__ == "__main__":
    ip="127.0.0.1"
    # port=6699
    # ip="192.168.3.41"
    ip="10.42.0.1"
    port=8866
    if len(sys.argv) < 3:
        help()
        # exit(1)
    else:
        ip = sys.argv[1]
        port = int(sys.argv[2])

    print("recv draw ip=%s port=%d"%(ip,port))
    impl = recv_obs(ip,port)
    impl.loop_run()
